// Field & Service Robotics
// Team 5
// Mars Manipulator
// code for the microcontroller to control:
// AS12X+ servo @ shoudler
// AS12X+ servo @ elbow
// AS12XA servo @ elbow
// linear actuator @ wrist location
// grip servo @ gripper

// feedback now being implemented

// Robert Sosa & Dorian Tsai
// Nov 16 2011
// Wrist Servo added Nov 29th 2011
// Reseting "function"; jumping to 0 not a real reset Nov 30th 2011

//#include <SoftwareSerial.h>
#include <ax12.h>
#include <string.h>
#include <ctype.h>
#include <Servo.h>
//PWMServo.h

// MESSAGE PROTOCOL:
// $As090e090v000g001;
// $As###e###v###g###;
// $ specifies the start of a new message
// A specifies actuator control
// --
// s specifies shoulder servo
// ### specifies the desired position in degrees
// e specifies elbow servo
// --
// v specifies the linear actuator
// ### specifies the desired position, from 0 (fully retracted) to 255 (fully extended)
// ex. v127; would tell the actuator to go to the halfway point
// --
// g specifies the gripper servo
// ### specifies the desired position in degrees
// ; specifies the end of the message
// ex. g090; would tell the servo to go to 90 degrees

#define MSG_END ';' // 59 = semi-colon
#define MSG_LENGTH 19 // maximum msg length
#define MSG_TYPE_ACT 'A' // actuator type msg => receive commands from computer and send to actuators, excluding gripper
#define MSG_TYPE_VRT 'V' // actuator type msg => move vertical actuator
#define MSG_TYPE_GRP 'G' // actuator type msg => move gripper
#define MSG_TYPE_MSR 'M' // get position type msg => get positions from actuators and send to computer
#define MSG_TYPE_STA '*' // starting msg type => don't do anything
#define MSG_TYPE_SPD 'S'
#define MSG_TYPE_UNKNOWN 'U'
#define TIME_LIMIT 2500 // time limit to wait for actuators to get into position
#define TIME_LIMIT_GRIP 500 // time limit to wait for actuators to get into position
#define TIME_LIMIT_VERT 7000 // time limit to wait for linear actuator
#define SPEED_DEFAULT 100 // default servo speed
#define DEBUG true // boolean for printing user-friendly statements or not


int ledPin = 13;

char msg[MSG_LENGTH]; // incoming serial 
char msgG[MSG_LENGTH + 6]; // outgoing get msg to serial

char msgStartAct[3] = "$A";
char msgStartGrip[3] = "$G";
char msgStartVert[3] = "$V";
char msgStartMsr[3] = "$M";
char msgStartSpeed[3] = "$S";

int conta=0;
int gen_counter = 0;
boolean msgEnd = true; // boolean to control while loop, signifying end of received serial msg
int msgPtr = 0; // index of incoming character array from serial comm

AX12 motor_s;
AX12 motor_e;
AX12 motor_w;
Servo gripServo; // define gripper servo object
Servo vertServo; // controlling linear actuator with servo object
int gripPin = 9; // grip Servo pin, PWM
int vertPin = 5; // linear actuator pin, PWN
int gripOutputPin = A4; // analog input pin for grip servo potentiometer readouts
int vertOutputPin = A0; // analog input pin for vertical actuator readout

int pGripRaw; // raw analog input of grip servo
int pVertRaw; // raw analog input of vertical actuator which comes as voltage

// joint positions of actuators
int pShdr; // shoulder position [deg]
int pElbw; // elbow position [deg]
int pWrst; // wrist position [deg]
int pVert; // vertical position [mm]
int pGrip; // gripper position [deg]
char pFSR; // Force Sensor Value (T = something is Taken (true), X = nothing is taken (false) 

// joint positions received from the computer 
int pShdrMove;
int pElbwMove;
int pWristMove;
int pVertMove;
int pGripMove;

int vS = SPEED_DEFAULT; // default servo speed for shoudler servo
int vE = SPEED_DEFAULT;  // default servo speed for elbow servo
int vW = SPEED_DEFAULT;
int servoSpeed = SPEED_DEFAULT;

unsigned long time; // var to hold number of milliseconds since the program started when millis() is called

//Force Sensor Resitor parameters
int fsrPin = A5;     // the FSR and 10K pulldown are connected to a0
int fsrReading;     // the analog reading from the FSR resistor divider
int fsrVoltage;     // the analog reading converted to voltage
unsigned long fsrResistance;  // The voltage converted to resistance, can be very big so make "long"
unsigned long fsrConductance; 
long fsrForce;       // Finally, the resistance converted to force


////////////////////// ======================== void setup =========================== /////////////////////////

void setup() {
  Serial.flush();
  Serial1.flush();
  motor_s = AX12();
  motor_e = AX12();
  motor_w = AX12();
  Serial.begin (115200); // initialize serial communication at high baud rate of 115200 b/s
  
  gripServo.attach(gripPin);  
  gripServo.write(120); // set gripServo to default position of 110 degrees
  vertServo.attach(vertPin);
  vertServo.writeMicroseconds(1000); // fully retract linear actuator 1000 us = retract to 2000 us = extend
  
  if (DEBUG) {
    Serial.println();
    Serial.println("===============Start Microcontroller - MARS Manipulator================");
    Serial.println();
  }
  establishContact(); // send a byte to establish contact until receiver responds                                      
  
  // initialize
  AX12::init (1000000);   // initialize AX12 at 1 Mb/s  
  if (DEBUG) {
    Serial.println();
    Serial.println("Initialize buffer");
  }
  for (int i=0; i<MSG_LENGTH; i++) {       // Initialize a buffer for received data
    msg[i]='*';
    msgG[i]=' ';
  }
  if (DEBUG) {
    Serial.println("Done");
  }

  byte detect[3];          
  byte num = AX12::autoDetect (detect, 3); // detect IDs from motors for naming later (not changing ID)
  
  if (DEBUG) {
    Serial.print ("  Detected: ");
    Serial.println (num, DEC);
  }

// tried reversing ID's after shoulder and wrist are switched
  motor_s.id = detect[2]; // Name the previously detected IDs to the motor instances.
  motor_e.id = detect[1];
  motor_w.id = detect[0];
  motor_s.SRL = RETURN_ALL;
  motor_e.SRL = RETURN_ALL;
  motor_w.SRL = RETURN_ALL;
  
  if (DEBUG) {
    Serial.print ("  Detected ID: ");
    Serial.println (detect[0], DEC);
    Serial.print ("  Detected ID: ");
    Serial.println (detect[1], DEC);
    Serial.print ("  Detected ID: ");
    Serial.println (detect[2], DEC);
  }
  AX12info test1 = motor_s.readInfo (RETURN_DELAY_TIME);
  AX12info test2 = motor_e.readInfo (RETURN_DELAY_TIME);
  AX12info test3 = motor_w.readInfo (RETURN_DELAY_TIME);
  if (DEBUG) {
    Serial.print (" delay: ");
    Serial.println (test1.value, DEC);
    Serial.print (" error: ");
    Serial.println (test1.error, DEC);
    Serial.print (" delay: ");
    Serial.println (test2.value, DEC);
    Serial.print (" error: ");
    Serial.println (test2.error, DEC);
    Serial.print (" delay: ");
    Serial.println (test3.value, DEC);
    Serial.print (" error: ");
    Serial.println (test3.error, DEC);
  }
  
  //int pos_s = motor_s.getPos();
  //int pos_e = motor_e.getPos();
  //Serial.print("Actual Shoulder angle is: ");
  //Serial.println(pos_s, DEC);
  //Serial.print("Actual Elbow angle is: ");
  //Serial.println(pos_e, DEC);
  
  pShdrMove = getShoulderPos();
  pElbwMove = getElbowPos();
  pWristMove = getWristPos();
  pVertMove = getVerticalPos();
  pGripMove = getGripperPos(); 
  
  vS = SPEED_DEFAULT;
  vE = SPEED_DEFAULT;
  vW = SPEED_DEFAULT;
  
}

///////////////////////////////////////////////////////////////////////////////////

void(* resetFunc) (void) = 0; //declare "fake reset" function @ address 0


////////////////////////////////////////////////////////////////////////////////////
void loop() {

  //digitalWrite(ledPin, HIGH);
  
  // receive input from computer
  // until we reach the terminating character (MSG_END) & the msg length reaches 
  // the max msg length, loop
  // whenever serial data comes in, receive it and
  // put the entire received msg into the msg character array
  
  // idea: timer to go for 0.5 sec, then clear/flush the buffer
  // we don't expect the same msg after a half second?
  while ((!msgEnd ))//&& (msgPtr < MSG_LENGTH)) 
  {
    if (Serial.available() > 0) {
      // wait for start byte? if Serial.read() == '$' and we haven't started reading yet...
      msg[msgPtr] = Serial.read(); // get incoming byte (a single character)
      
      if (msg[msgPtr] == '$'){
        msg[0] = msg[msgPtr];
        msgPtr = 1;      
      }
      else if (msg[msgPtr] == MSG_END) {
          msgEnd = true;      
      } 
      else {
         msgEnd = false;      
         msgPtr++;
      }
    } 
    // NOTE: NEEDS ERROR HANDLING
  }
  
  //Call reset function in case of RST; string
  if (msg[1] == 'R' && msg[2] == 'S' && msg[3] == 'T') {
    Serial.println("Reset microcontroller");
    resetFunc();  //call reset
  }
  
  // if we have reached the end of msg successfully
  if (msgEnd == true) 
  { 
    // check to see correct msg has been received (sends the entire msg back to serial)
    if (DEBUG) {
      Serial.println();
      Serial.print("Received message: ");
    }
    Serial.print(msg);
    Serial.println();

	////////////////////////============== DETERMINE TYPE ===========//////////////////////////////
    // allow for other types of msg types in future
    char type;
    if (msg[0] == msgStartAct[0] && msg[1] == msgStartAct[1]) {
      type = MSG_TYPE_ACT;
    } else if (msg[0] == msgStartMsr[0] && msg[1] == msgStartMsr[1]) {
        type = MSG_TYPE_MSR;
    } else if (msg[0] == msgStartVert[0] && msg[1] == msgStartVert[1]) {
        type = MSG_TYPE_VRT;
    } else if (msg[0] == msgStartGrip[0] && msg[1] == msgStartGrip[1]) {
        type = MSG_TYPE_GRP;
    } else if (msg[0] == msgStartSpeed[0] && msg[1] == msgStartSpeed[1]) {
        type = MSG_TYPE_SPD;
    } else if (msg[0] == '*' && msg[1] == '*') { // if starting message (all "****..."), do nothing!
      for (int i=0; i<MSG_LENGTH; i++) {
        if (msg[i] == '*') {
          type = MSG_TYPE_STA;
        }
      }
    } else {
      type = MSG_TYPE_UNKNOWN;
      if (DEBUG) {
        Serial.println("ERROR: type unknown. It was not ACT or GET");
      }
      else {
        Serial.print('E1');
        Serial.println();
      }
    }
	
    //////////////////-------------------- ACT --------------------------------////////////////////
    if (type == MSG_TYPE_ACT) { // Reads data from the array
      //Serial.println("Is Actuator command");         
      // $As###e###w###;
      // 0123456789
      //shoulder_angle(3);
      //elbow_angle(7);
   
      for (int i=2; i<msgPtr+1; i++) {
        if (msg[i]=='s'){    // check for the position of the  "," separator or the new update is looking for the command and not commas
           gen_counter = i;
           pShdrMove = atoi(&msg[gen_counter +1]);
           pShdrMove = pShdrMove - 90; // minus 90 because computer added 90 to avoid negative numbers
           shoulder_angle();
           i = i + 3; // command number is 3 characters
        }
        else if (msg[i]== 'e'){
           gen_counter = i; 
           pElbwMove = atoi(&msg[gen_counter +1]);
           pElbwMove = pElbwMove - 115; // subtract 115 because 115 was added to angles
           //pElbwMove = -pElbwMove; // invert because of servo positioning, placed in code later so that we still see positive values
           elbow_angle();
           i = i + 3; 
        }
        else if (msg[i]== 'w'){
          gen_counter = i; 
          pWristMove = atoi(&msg[gen_counter +1]);
          pWristMove = pWristMove - 90; // subtract 90 because 90 was added to angles
          //pWristMove = -pWristMove; // invert because of servo positioning, placed in code later so that we still see positive values
          wrist_angle();
          i = i + 3; 
        }
        else if (msg[i] == MSG_END) {
          
          if (DEBUG) {
            Serial.println("Loop until servos reach commanded position");
          }
          time = millis(); // returns the number of milliseconds since Arduino board began running current program
          unsigned long dTime = 0;
          int marginSEW = 2; // deg
          boolean s = false;
          boolean e = false;
          boolean w = false;
          boolean reachPos = false;
            
          while (dTime < (TIME_LIMIT) && reachPos == false) {
            // get current positions
            pShdr = getShoulderPos();
            pElbw = getElbowPos();
            pWrst = getWristPos();
                         
            // compare to move positions
            if ((pShdr >= pShdrMove-marginSEW) && (pShdr <= pShdrMove+marginSEW)) {
              s = true;
            }
            if ((pElbw >= pElbwMove-marginSEW) && (pElbw <= pElbwMove+marginSEW)) {
              e = true;
            }
            if ((pWrst > pWristMove-marginSEW) && (pWrst < pWristMove+marginSEW)) {
              w = true;
            }
    
              // if match within a margin then reachPos == true then send "$Done" msg
            if (s && e && w) {
              reachPos = true;
              delay(500);
              // send Tilman msg "$Don$
              if (DEBUG) {
                Serial.println("Servos position reached");
                Serial.println("Sending D message: ");
              }
              Serial.println("$D;");
            }
                         
            dTime = millis() - time;
            if (dTime >= TIME_LIMIT) {
              Serial.println("====TIME OUT FOR SERVO POS====");
              Serial.print("pShdrMove = ");
              Serial.print(pShdrMove);
              Serial.print(" pShdr = ");
              Serial.println(pShdr);
              
              Serial.print("pElbwMove = ");
              Serial.print(pElbwMove);
              Serial.print(" pElbw = ");
              Serial.println(pElbw);
                
              Serial.print("pWristMove = ");
              Serial.print(pWristMove);
              Serial.print(" pWrst = ");
              Serial.println(pWrst);
              
              //Serial.println("Try to reset uC");
              //resetFunc();  //call reset
              Serial.println("$E;");
           }
           
           // clear s,e,w at end of loop to ensure that all are true at the same point in time
           //s = false;
           //e = false;
           //w = false;
           
         } // end while loop for reaching position
            //delay(250); // wait for actuator commands to follow through
        }
        else {
          if (DEBUG) {
             Serial.println('End After End Msg');
          }
	}  // end for loop of msg reading
      } 
	  /* depracated - error handling if inside msg act was all garbled
      else {
        if (DEBUG) {
            Serial.println("ERROR: No recognizable Actuator command");
          }
        else {
          Serial.print('E2');
          Serial.println();
        }
      }*/
    } /////////////////////////// end actuator type msg ///////////////////////////
//////////////////----------------------- VERT -------------------------------////////////////////
    else if (type == MSG_TYPE_VRT) { // Reads data from the array
      //Serial.println("Is Actuator command");         
      // $As###e###v###g###;
      // 0123456789
      //shoulder_angle(3);
      //elbow_angle(7);
      //vertical_position(11);
      //gripper_angle(15);
   
      for (int i=2; i<msgPtr+1; i++){
        if (msg[i]== 'v'){
           gen_counter = i;
           pVertMove = atoi(&msg[gen_counter +1]);
           vertical_position();
           i = i + 3;
        } 
        else if (msg[i] == MSG_END){
          
          if (DEBUG) {
            Serial.println("End of msg, loop until vertical reach commanded position");
          }
            
          time = millis(); // returns the number of milliseconds since Arduino board began running current program
          unsigned long dTime = 0;
          int marginV = 4; // mm
          boolean v = false;
          boolean reachPos = false;
           
          //while (dTime < TIME_LIMIT_VERT && reachPos == false) 
          while (dTime < TIME_LIMIT_VERT) {
            // get current positions
            pVert = getVerticalPos();           
            /*
            // compare to move positions
            if ((pVert >= pVertMove-marginV) && (pVert <= pVertMove+marginV)) {
              v = true;
              //Serial.println("V reached");
            }
            
            // if match within a margin then reachPos == true then send "$Done" msg
            if (v) {
              reachPos = true;
              if (DEBUG) {
                Serial.println("Vertical position reached");
                Serial.println("Sending D message: ");
              }
              Serial.println("$D;");
            }*/
             
            dTime = millis() - time;
            if (dTime >= TIME_LIMIT_VERT) {
              //Serial.println("Time's up");
              
              Serial.print("pVertMove = ");
              Serial.print(pVertMove);
              Serial.print(" pVert = ");
              Serial.println(pVert);
              
              Serial.println("$D;");
              //Serial.println("$E;");
            }
          }
            //delay(250); // wait for actuator commands to follow through
        }
        else {
          Serial.print('End');
          Serial.println();
        }
      }
    }     
    //////////////////-------------------- GRIP -------------------------------////////////////////
    else if (type == MSG_TYPE_GRP) { // Reads data from the array
      //Serial.println("Is Actuator command");         
      // $As###e###v###g###;
      // 0123456789
      //shoulder_angle(3);
      //elbow_angle(7);
      //vertical_position(11);
      //gripper_angle(15);
   
      for (int i=2; i<msgPtr+1; i++){
        if (msg[i]== 'g'){
           gen_counter = i;
           pGripMove = atoi(&msg[gen_counter +1]);
           gripper_angle(); 
           i = i + 3;
        } 
        else if (msg[i] == MSG_END){
          
          if (DEBUG) {
            Serial.println("End of msg, loop until gripper reach commanded position");
          }
		  
          time = millis(); // returns the number of milliseconds since Arduino board began running current program
          unsigned long dTime = 0;
          int marginG = 5; // deg
          boolean g = false;
          boolean reachPos = false;
            
          //while (dTime < TIME_LIMIT_GRIP && reachPos == false) {
          while (dTime < TIME_LIMIT_GRIP) {
             
            // get current positions
            pGrip = getGripperPos();              
            
            // compare to move positions
            //if ((pGrip >= pGripMove-marginG) && (pGrip <= pGripMove+marginG)) {
            //  g = true;
              //Serial.println("G reached");
            //}
             
            // if match within a margin then reachPos == true then send "$Done" msg
            // if (s && e && w && v && w && g) {
            //if (g) {
            //  reachPos = true;
            //  delay(200);
            //  if (DEBUG) {
            //    Serial.println("Sending D message: ");
            //    Serial.println("Gripper position reached");
            //  }
            //  Serial.println("$D;");
            //}            
              
            dTime = millis() - time;
            if (dTime >= TIME_LIMIT_GRIP) {
              Serial.println("====TIME OUT FOR GRIP POS - works ====");
              
              //Serial.print("pGripMove = ");
              //Serial.print(pGripMove);
              //Serial.print(" pGrip = ");
              //Serial.println(pGrip);
               Serial.println("$D;");
            }
          }
            //delay(250); // wait for actuator commands to follow through
        }
        else {
          Serial.print('End');
          Serial.println();
        }
      } 
      /*else {
        if (DEBUG) {
          Serial.println("ERROR: No recognizable command");
        }
        else {
          Serial.print('E2');
          Serial.println();
        }
      }*/
    } /////////////////////////// end gripper type msg ///////////////////////////
    //////////////////----------------------- SPEED -------------------------------////////////////////
    else if (type == MSG_TYPE_SPD) { 
      
      for (int i=1; i<msgPtr+1; i++){
        if (msg[i]== 'S'){
           gen_counter = i;
           servoSpeed = atoi(&msg[gen_counter +1]);
           
           i = i + 3;
        } 
        else if (msg[i] == MSG_END){
          
          if (DEBUG) {
            Serial.println("End of msg, now set servo speeds");
          }
          
          if (servoSpeed > 250) 
            servoSpeed = 250;
          if (servoSpeed < 1)
            servoSpeed = 1;
            
          vS = servoSpeed;
          vE = servoSpeed;
          vW = servoSpeed;
          Serial.print("Servo speeds set to ");
          Serial.print(servoSpeed);
          Serial.println();
          
          //motor_s.setVel(servoSpeed);
          //motor_e.setVel(servoSpeed);
          //motor_w.setVel(servoSpeed);
          
          
          
            //delay(250); // wait for actuator commands to follow through
        }
        else {
          Serial.print('End');
          Serial.println();
        }
      }
    }     
    //////////////=================== get message type ===================/////////////////
    else if (type == MSG_TYPE_MSR) {
      // get positions of all actuators 
      
      pShdr = getShoulderPos();
      pElbw = getElbowPos();
      pWrst = getWristPos();
      pVert = getVerticalPos();
      pGrip = getGripperPos();
      pFSR = readFSR();
      
      if (DEBUG) {
        Serial.println("Got: ");
        Serial.print("pShdr = "); Serial.println(pShdr);
        Serial.print("pElbw = "); Serial.println(pElbw);
        Serial.print("pWrst = "); Serial.println(pWrst);
        Serial.print("pVert = "); Serial.println(pVert);
        Serial.print("pGrip = "); Serial.println(pGrip);
        Serial.print("pFSR = "); Serial.println(pFSR);
      }
      
      
      // send positions of all actuators to computer (print)
      // send as $Gs###e###v###g###;
      // add 90/115 to received position of elbow and shoulder to avoid minus signs.
      pShdr = pShdr + 90;
      pElbw = pElbw + 115;
      pWrst = pWrst + 90;
      
      // decompose positions to arrays wh$As090e090v000g090;ich hold each digit
      int pShArr[3];
      pShArr[0] = (pShdr - (pShdr % 100))/100 + 48; // add 40 for '0' ASCII char offset
      pShArr[1] = (pShdr % 100 - pShdr % 10)/10 + 48;
      pShArr[2] = pShdr % 10 + 48;
      int pElArr[3];
      pElArr[0] = (pElbw - (pElbw % 100))/100 + 48;
      pElArr[1] = (pElbw % 100 - pElbw % 10)/10 + 48;
      pElArr[2] = pElbw % 10 + 48;
      int pWrArr[3];
      pWrArr[0] = (pWrst - (pWrst % 100))/100 + 48;
      pWrArr[1] = (pWrst % 100 - pWrst % 10)/10 + 48;
      pWrArr[2] = pWrst % 10 + 48;
      int pVrArr[3];
      pVrArr[0] = (pVert - (pVert % 100))/100 + 48;
      pVrArr[1] = (pVert % 100 - pVert % 10)/10 + 48;
      pVrArr[2] = pVert % 10 + 48;
      int pGrArr[3];
      pGrArr[0] = (pGrip - (pGrip % 100))/100 + 48;
      pGrArr[1] = (pGrip % 100 - pGrip % 10)/10 + 48;
      pGrArr[2] = pGrip % 10 + 48;
      
      // combine all number arrays into one large character array according to protocol
      msgG[0] = (char) '$';
      msgG[1] = (char) 'U';
      msgG[2] = (char) 's';
      msgG[3] = (char) pShArr[0];
      msgG[4] = (char) pShArr[1];
      msgG[5] = (char) pShArr[2];
      msgG[6] = (char) 'e';
      msgG[7] = (char) pElArr[0];
      msgG[8] = (char) pElArr[1];
      msgG[9] = (char) pElArr[2];
      msgG[10] = (char) 'v';
      msgG[11] = (char) pVrArr[0];
      msgG[12] = (char) pVrArr[1];
      msgG[13] = (char) pVrArr[2];
      msgG[14] = (char) 'w';
      msgG[15] = (char) pWrArr[0];
      msgG[16] = (char) pWrArr[1];
      msgG[17] = (char) pWrArr[2];
      msgG[18] = (char) 'g';
      msgG[19] = (char) pGrArr[0];
      msgG[20] = (char) pGrArr[1];
      msgG[21] = (char) pGrArr[2];
      //msgG[22] = (char) ';';
      msgG[22] = (char) 'f';
      msgG[23] = (char) pFSR;   //T (for taken) or X (not taken)
      msgG[24] = (char) ';';
      
      if (DEBUG) {
		Serial.println("Sending U message:");
      }
      Serial.print(msgG); // should send to computer
      Serial.println();
    }
    else if (type == MSG_TYPE_STA) {
      // Serial.println("Start msg");
      // do nothing
    }
    
    // else if (other types of msgs)  do other stuff
    
  }// end of msg true 
  else if (msg[0] == ' ' && msg[1] == ' ' && msg[2] == ' ') {
    // basically, if the msg is empty, do nothing - we are waiting for the next msg
    // Serial.println("Msg empty - do nothing.");
  }
  else {
    
    if (DEBUG) {
      Serial.print("ERROR: Unkown msg - ");
      Serial.print("Message: ");
      Serial.println(msg);
    }
    else {
      Serial.print('E3');
      Serial.println();
    }
  }
  
  // reset variables for loop
  msgEnd = false;
  msgPtr = 0;
  
  for (int i = 0; i < MSG_LENGTH + 6; i++) 
  {
    msg[i] = ' ';
    msgG[i] = ' ';
  } 
}

////////////////////////////////////////////////////////////////////////////////////////////

void shoulder_angle() {   
  
  if (pShdrMove > 90 || pShdrMove < -90) {
    //Serial.print('Es');
    //Serial.println();
    if (DEBUG) {
      Serial.println("ERROR: angle_s outside shoulder range! Abort shoulder rotate!");  
    }
  } else {
    if (DEBUG) {
      Serial.print("Shoulder: ");
      Serial.print(pShdrMove);
      Serial.println();
    }
    
    motor_s.setVel(vS);
    
    int pShdrMapped = map(pShdrMove,-150,150,0,1023);
    motor_s.setPos(pShdrMapped);
    
    //delay(50);
    // check that we've reached position. If not, try again. This was a common issue with AS-12+ servos.
    int pShdrCheck = motor_s.getPos(); 
    if (pShdrCheck == pShdrMapped){
      return; // means we're at the right position. Note that there may be a few +- differences
    }
    else {
      if (DEBUG) {
        //Serial.println("Shoulder: Second attempt at mapping angle!");
      }
      motor_s.setPos(pShdrMapped); // try to set motor position again!
     motor_s.setPos(pShdrMapped); // try to set motor position again!
     motor_s.setPos(pShdrMapped); // try to set motor position again!
     motor_s.setPos(pShdrMapped); // try to set motor position again!
    }
  }
  
    //int vel = motor.getSpeed(); 
    //int load = motor.getLoad();
    //delay(100);
    //Serial.println (" ");
    //Serial.print (" Shoulder: ");
    //Serial.println (pos, DEC);
    //Serial.print (" velocidad: ");
    //Serial.println (vel, DEC);
    //Serial.print (" carga: ");
    //Serial.println (load, DEC);
}

void elbow_angle(){
  

  if (pElbwMove > 115 || pElbwMove < -115) {
    //Serial.print('Ee');
    //Serial.println();
    if (DEBUG) {
      Serial.println("ERROR: angle_e outside elbow range! Abort elbow rotate!");  
    }
  } else {
    // just to check if the correct command was received/converted
    if (DEBUG) {
      Serial.print("Elbow: ");
      Serial.print(pElbwMove);
      Serial.println();
    }
    motor_e.setVel(vE);
    
    
    int pElbwMapped = map(pElbwMove,150,-150,0,1023);
    motor_e.setPos(pElbwMapped);
    
    //delay(100);
    
    int pElbwCheck = motor_e.getPos(); 
    if (pElbwCheck == pElbwMapped){
       return;
    }
    else {
      motor_e.setPos(pElbwMapped);
      motor_e.setPos(pElbwMapped);
      motor_e.setPos(pElbwMapped);
      motor_e.setPos(pElbwMapped);
      if (DEBUG) {
       // Serial.println("Elbow: Second attempt at mapping angle!");
      }
    } 
  }  
}


void wrist_angle(){
  
  if (pWristMove > 90 || pWristMove < -90) {
    //Serial.print('Ew'); 
    //Serial.println();
    if (DEBUG) {
      Serial.println("ERROR: angle_w outside wrist range! Abort wrist rotate!");  
    }
  } else {
    // just to check if the correct command was received/converted
    if (DEBUG) {
      Serial.print("Wrist: ");
      Serial.print(pWristMove);
      Serial.println();
    }
    motor_w.setVel(vE);
    
    
    int pWristMapped = map(pWristMove,150,-150,0,1023);
    motor_w.setPos(pWristMapped);
    
    //delay(100);
    
    int pWristCheck = motor_w.getPos(); 
    if (pWristCheck == pWristMapped){
       return;
    }
    else {
      motor_w.setPos(pWristMapped);
      motor_w.setPos(pWristMapped);
      motor_w.setPos(pWristMapped);
      motor_w.setPos(pWristMapped);
      if (DEBUG) {
        //Serial.println("Wrsit: Second attempt at mapping angle!");
      }
    } 
  }
}


void vertical_position() {
  if (pVertMove < 0 || pVertMove > 140) {
    //Serial.print('Ev');
    //Serial.println();
    if (DEBUG) {
      Serial.print("ERROR: pVert outside linear actuator range! Abort vertical!");  
    }
  } else {
    // just to check if the correct command was received/converted
    if (DEBUG) {
      Serial.print("Linear actuator: ");
      Serial.print(pVertMove);
      Serial.println();
    }
    
    // write position to servo
    //analogWrite(vertPin,pVertMove);
    int pVertMoveMap = map(pVertMove,0,140,1000,2000);
    //pVertMoveMap = map(pVertMoveMap,100,200,1000,2000);
    
    //vertServo.write(pVertMoveMap);
    vertServo.writeMicroseconds(pVertMoveMap);
    
    //int pVertCheck = getVerticalPos();
    //Serial.print(pVertCheck); Serial.println();
    
    /*if (pVertCheck == pVertMove) {
      // everything is fine
    } 
    else {
      if (DEBUG) {
        Serial.println("Vert: try again");
      }
      vertServo.write(pVertMoveMap); // try again
    }*/
  }
}

void gripper_angle(){


  if (pGripMove < 80 || pGripMove > 140) {
    //Serial.print('Eg');
    //Serial.println();
    if (DEBUG) {
      Serial.print("ERROR: pGrip outside servo range (80,140)!");  
    }
  } else {
    // just to check if the correct command was received/converted
    if (DEBUG) {
      Serial.print("Grip servo: ");
      Serial.print(pGripMove);
      Serial.println();
    }
    // write position to servo
    gripServo.write(pGripMove);
    
    /*
    delay(1000); // need some time for poteniometer to catch up to current position
    
    int pGripCheck = getGripperAngle(); 
    if (pGripCheck == pGripMove) {
    //if ( (pGripCheck > (pGripMove)) && (pGripCheck > (pGripMove+2)) ){
      Serial.println("G - is within range. Don't write again");
      return;
    }
    else {
      gripServo.write(pGripMove);
      if (DEBUG) {
        //Serial.println("Gripper: Second attempt at mapping angle!");
      }
    }*/
  }
}

int getShoulderPos() {
//pShdr = 10;
      int pShdrTemp1 = map(motor_s.getPos(),0,1023,-150,150);
      int pShdrTemp2 = map(motor_s.getPos(),0,1023,-150,150);
      int pShdrTemp3 = map(motor_s.getPos(),0,1023,-150,150);
      // int pShdrTemp = map(pShdrRaw,0,1023,-150,150);
      
      int pShdrTemp = (abs(pShdrTemp1) + abs(pShdrTemp2) + abs(pShdrTemp3))/3;
      int tally = 0;
      if (pShdrTemp1 > 0) {
        tally = tally + 1;
      } else {
        tally = tally - 1;
      }
      if (pShdrTemp2 > 0) {
        tally = tally + 1;
      } else {
        tally = tally - 1;
      }
      if (pShdrTemp3 > 0) {
        tally = tally + 1;
      } else {
        tally = tally - 1;
      }
      if (tally < 0) {
        pShdrTemp = -pShdrTemp; // negative
      }
      
      
      if ((pShdrTemp > 0) || (pShdrTemp < 0)) {
        pShdrTemp = pShdrTemp + 1; // add one because get motor is always -1 deg off
      }
      if (DEBUG) {
        //Serial.print("Got Shoulder: ");
        //Serial.print(pShdr);
        //Serial.println();
      }
      return pShdrTemp;
}

int getElbowPos() {
      //pElbw = 20;
      int pElbwTemp1 = map(motor_e.getPos(),0,1023,150,-150);
      int pElbwTemp2 = map(motor_e.getPos(),0,1023,150,-150);
      int pElbwTemp3 = map(motor_e.getPos(),0,1023,150,-150);
      int pElbwTemp = (abs(pElbwTemp1) + abs(pElbwTemp2) + abs(pElbwTemp3))/3;
      int tally = 0;
      if (pElbwTemp1 > 0) {
        tally = tally + 1;
      } else {
        tally = tally - 1;
      }
      if (pElbwTemp2 > 0) {
        tally = tally + 1;
      } else {
        tally = tally - 1;
      }
      if (pElbwTemp3 > 0) {
        tally = tally + 1;
      } else {
        tally = tally - 1;
      }
      if (tally < 0) {
        pElbwTemp = -pElbwTemp; // negative
      }
      
      //int pElbwTemp = map(pElbwRaw,0,1023,150,-150); // note the reversed mapping because elbow joint is mounted in other direction
      
      //pElbwTemp = pElbwTemp + 1; // add one because get motor is always -1 deg off
      //pElbwTemp = -pElbwTemp;
      if (DEBUG) {
        //Serial.print("Got Elbow: ");
        //Serial.print(pElbw);
        //Serial.println();
      }
      return pElbwTemp;
}
      

int getWristPos() {
        //pWrst = 20;
      int pWrstTemp1 = map(motor_w.getPos(),0,1023,150,-150);
      int pWrstTemp2 = map(motor_w.getPos(),0,1023,150,-150);
      int pWrstTemp3 = map(motor_w.getPos(),0,1023,150,-150);
      int pWrstTemp = (abs(pWrstTemp1) + abs(pWrstTemp2) + abs(pWrstTemp3))/3;
      int tally = 0;
      if (pWrstTemp1 > 0) {
        tally = tally + 1;
      } else {
        tally = tally - 1;
      }
      if (pWrstTemp2 > 0) {
        tally = tally + 1;
      } else {
        tally = tally - 1;
      }
      if (pWrstTemp3 > 0) {
        tally = tally + 1;
      } else {
        tally = tally - 1;
      }
      if (tally < 0) {
        pWrstTemp = -pWrstTemp; // negative
      }
      
      //int pWrstTemp = map(pWrstRaw,0,1023,150,-150); // note the reversed mapping because Wrist joint is mounted in other direction
      
      //pWrstTemp = pWrstTemp + 1; // add one because get motor is always -1 deg off
      //pWrstTemp = -pWrstTemp;
      if (DEBUG) {
        //Serial.print("Got Wrist: ");
        //Serial.print(pWrst);
        //Serial.println();
      }
      return pWrstTemp;
}


int getVerticalPos() {
  //pVertRaw = 3.3;
      //Serial.println("Servo read");
      //Serial.println();
      //Serial.println();
      int pVertRaw = vertServo.read();// reads the last instruction sent to the actuator
      
      
      
      //int pVertRaw = analogRead(vertOutputPin); // input in volts?
      //pVert = map(pVertRaw,
      //pVert = map(pVertRaw,00,667,200,100); // map to nominal vertical output
      int pVertTemp = map(pVertRaw,44,141,0,140); // [mm]
      if (pVertTemp < 0) {
        pVertTemp = 0; // a quick work-around solution, since sometimes the mapping is a little bit off and produces negative values
      }
      else if (pVertTemp > 140) {
        pVertTemp = 140;
      }
      if (DEBUG) {
        //Serial.print("Got Vert Raw: ");
        //Serial.print(pVertRaw);
        //Serial.println();
        //Serial.print("Got Vert: ");
        //Serial.print(pVert);
        //Serial.println();
      }
  return pVertTemp;
}

int getGripperPos() {
  //pGripRaw = 1023;
  int pGripRaw = 0;
  int avgWindow = 3;
  /*
  for (int i = 1; i <= avgWindow; i++) {
    pGripRaw = pGripRaw + analogRead(gripOutputPin);
  }
  pGripRaw = pGripRaw/avgWindow; // average several analog measurements
  int pGripRaw1 = analogRead(gripOutputPin); // in raw poteniometer bits
  int pGripTemp = map(pGripRaw,327,327,80,140); // convert to degrees. Calibration is key!
  */
  int pGripTemp = gripServo.read();
  if (DEBUG) {
    //Serial.print("Got Gripper Raw: ");
    //Serial.print(pGripRaw);
    //Serial.println();
    //Serial.print("Got Gripper Raw: ");
    //Serial.print(pGripRaw1);
    //Serial.println();
    //Serial.print("Got Gripper: ");
    //Serial.print(pGripTemp);
    //Serial.println();
  }
  return pGripTemp;
}

char readFSR(){
  char grabbed = 'T';
  fsrReading = analogRead(fsrPin);  
  //Serial.print("Analog reading = ");
  //Serial.println(fsrReading);
 
  // analog voltage reading ranges from about 0 to 1023 which maps to 0V to 5V (= 5000mV)
  fsrVoltage = map(fsrReading, 0, 1023, 0, 5000);
  //Serial.print("Voltage reading in mV = ");
  //Serial.println(fsrVoltage);
  
  if (fsrVoltage <= 2000) {
    //Serial.println("No pressure");
  grabbed = 'X';  
  } else {
    // The voltage = Vcc * R / (R + FSR) where R = 10K and Vcc = 5V
    // so FSR = ((Vcc - V) * R) / V        yay math!
    //fsrResistance = 5000 - fsrVoltage;     // fsrVoltage is in millivolts so 5V = 5000mV
    //fsrResistance *= 10000;                // 10K resistor
    //fsrResistance /= fsrVoltage;
    //Serial.print("FSR resistance in ohms = ");
    //Serial.println(fsrResistance);
 
    //fsrConductance = 1000000;           // we measure in micromhos so 
    //fsrConductance /= fsrResistance;
    //Serial.print("Conductance in microMhos: ");
    //Serial.println(fsrConductance);
 
    // Use the two FSR guide graphs to approximate the force
    //if (fsrConductance <= 1000) {
      //fsrForce = fsrConductance / 80;
      //Serial.print("Force in Newtons: ");
      //Serial.println(fsrForce);      
    //} else {
      //fsrForce = fsrConductance - 1000;
      //fsrForce /= 30;
      //Serial.print("Force in Newtons: ");
      //Serial.println(fsrForce);            
    //}
    grabbed = 'T';
  }
  return grabbed;
}


// send a byte to establish contact until receiver responds                                      
void establishContact() {
  int count = 0;
  while (Serial.available() <= 0) {
    count = count+1;
    if (count > 60) {
      Serial.println();
      count = 0;
    }
    Serial.print('A', BYTE);   // send a capital A
    delay(300);
  }
}


